#pragma once

#include <cv.h>
#include <highgui.h>
#include <math.h>
#include <vector>

using namespace std;

class Utils
{
public:
	Utils();
	~Utils();

	float CalCCOEF(CvMat* a, CvMat* b);

	float ComputeDistance(vector<float> a, vector<float> b);

	static bool isNaN( double r )
	{
		return r != r;
	}

	static double ComputeVariance(float a, float b) {
		float mean = (a + b) / 2;
		return ((a - mean)*(a - mean) + (b - mean)*(b - mean)) / 2;	
	}

	static double ComputeStandardDev(float a, float b) {
		return sqrt(Utils::ComputeVariance(a,b));
	}

	static float map(float v, float min1, float max1, float min2, float max2)
	{
		return (v - min1) * max2 / (max1 - min1) + min2;
	}

	static float CalGaussian(CvMat* x, CvMat* u, CvMat* sigma)
	{
		int d = x->cols;
		CvMat* S = cvCreateMat(d, d, CV_32FC1);
		cvZero(S);

		for (int i = 0; i < d; i++)
		{
			*((float*)CV_MAT_ELEM_PTR(*S, i, i)) = CV_MAT_ELEM(*sigma, float, 0, i);
		}

		float detVal = (float)sqrt(cvDet(S));
		float expr1 = pow(2*3.14, d/2) * detVal;
		expr1 = 1/expr1;

		CvMat* temp1 = cvCreateMat(1, d, CV_32FC1);
		CvMat* temp2 = cvCreateMat(1, d, CV_32FC1);
		CvMat* temp3 = cvCreateMat(d, 1, CV_32FC1);
		CvMat* Sinv = cvCreateMat(d, d, CV_32FC1);
		cvSub(x, u, temp1);
		cvInvert(S, Sinv);
		cvMatMul(temp1, Sinv, temp2);

		cvTranspose(temp1, temp3);
		CvMat* expr2 = cvCreateMat(1, 1, CV_32FC1);
		cvMatMul(temp2, temp3, expr2);
		float fExpr2 = CV_MAT_ELEM(*expr2, float, 0, 0);
		fExpr2 = fExpr2/(-2);
		fExpr2 = exp(fExpr2);

		float G = 0;
		G = expr1*fExpr2;
		return G;
	}
};